#include <QtGui>
#include <QColor>
#include "Spreadsheet.h"
#include <vector>
#include <string>
//#include ""
using namespace std;
#include "Matrices.h"
#include "PhiGamma.h"
//#include "CVSElapsedTimer.h"
#include "RoomData.h"
#include "Helper.h"
#include "FileIO.h"
class CFileIO;
#include "Disturbance.h"
class CDisturbance;

CRoomData::CRoomData()
:CPathData()
{
    _strClassName = "CRoomData";
}

CRoomData::~CRoomData(void)
{
    clear();
}

void CRoomData::load(const vector<CSpreadsheet*> &vSpreadsheet_ )
{
    clear();

    CData::load(vSpreadsheet_);

    //load init file
    {
        CSpreadsheet *pInit;
        for(unsigned int i=0;i<vSpreadsheet_.size();i++)
        {
            if(vSpreadsheet_[i]->currentFile().contains(".init"))
                pInit = vSpreadsheet_[i];
        }
        if(!pInit)
        {
            QString sError("Init File : can not find any init file.\n \"");
            sError += pInit->currentFile();
            sError += "\".\n";
            throw exception(sError.toStdString().c_str());
        }
        vector<double> vIn;
        pInit->search("HeadingBound",1,&vIn);         setHeadingBoundDegree() = vIn[0];
        pInit->search("TargetRadius",1,&vIn);         setTargetRadius() = vIn[0];
        //pInit->search("Target Always On Top",1,&vIn); setTargetAlwaysOnTop() = vIn[0];
        //pInit->search("In front of Far",1,&vIn);      setInfrontofFar() = vIn[0];
        pInit->search("Bounds of HeadingOffset",2,&vIn);   setHeadingOffsetBoundsDeg() = vIn;
        pInit->search("Bounds of GazeOffset",2,&vIn); setGazeOffsetBoundsDeg() = vIn;
        pInit->search("TargetColor",3,&setTargetColor());
        pInit->search("TargetPostColor",3,&setTargetPostColor());
        pInit->search("TargetPostPosition",3,&setTargetPostPosition());
        pInit->search("PostRadius",1,&vIn); setPostRadius() = vIn[0];
        pInit->search("PostHeight",1,&vIn); setPostHeight() = vIn[0];

        pInit->search("GroundColor",4,&setGroundColor());
        pInit->search("GazePerturbationAxisOffset",1,&vIn); setGazePerturbationAxisOffsetRad() = vIn[0]*DEG2RAD;
        pInit->search("SphereDiscRadius",1,&vIn); setSphereDiscRadius() = vIn[0];
        pInit->search("Bounds of GazeOffset",2,&vIn); setGazeOffsetBoundsDeg() = vIn;
        pInit->search("GroundColor",4,&setGroundColor());
        pInit->search("SphereDiscRadius",1,&vIn); setSphereDiscRadius() = vIn[0];
        pInit->search("numSquares",1,&vIn);           setNumSquares() = vIn[0];
        pInit->search("DotColor",4,&setDotColor());
        pInit->search("DotSize",1,&vIn);              setDotSize() = vIn[0];

        _vpPhiGamma.push_back(new CPhiGamma);
        _vpPhiGamma.push_back(new CPhiGamma);
        _vpPhiGamma[0]->load(vSpreadsheet_,"velocity.mtx");
        _vpPhiGamma[1]->load(vSpreadsheet_,"acceleration.mtx");
    }

    //check output file
    try
    {
        CheckOutputFile();
    }
    catch(exception& e)
        //__finally
    {
#if _DEBUG
        CFileIO cFIO;
        // check existence data directory and create if necessary
        cFIO.CreateCWDir(string("\\data")); // do not add trailing slash! (prefixing a slash is not necessary but ok
        int nResult = QMessageBox::question(NULL,QString("Warning"),QString(e.what())+QString("\nDo you want to remove these files?"),QMessageBox::Yes|QMessageBox::No,QMessageBox::Yes); 
        if(QMessageBox::Yes==nResult)
        {
            for(int nTrialIdx=0; nTrialIdx< GetRunScript().Rows(); nTrialIdx++)
            {
                QString qstrFullOuptputFileName;
                fullOutputFileName(nTrialIdx,&qstrFullOuptputFileName);
                cFIO.DelFile(qstrFullOuptputFileName.toStdString().c_str());
            }
        }
        else if(QMessageBox::No==nResult)
        {
            throw exception("Quit");
        }
#else
        string strError = "Data File already Exits.";
               strError += e.what();
        throw exception(strError.c_str());
#endif
    }
    return;
}

void CRoomData::updateOutput(int nJoyStickOff_/*=0*/)
{
    int nCurrFrameIdx = timeIdx();
    // read current joystick position from Flybox and save it in data class
    double dCurrControlState = 0;
    if(!nJoyStickOff_)
    {
        dCurrControlState = xPosition();
    }

    int nControlOrder = GetRunScript()(trialIdx(),0);
    if(1==nControlOrder)
        _vpPhiGamma[0]->UpdateOutput(nCurrFrameIdx,dCurrControlState);
    if(2==nControlOrder)
        _vpPhiGamma[1]->UpdateOutput(nCurrFrameIdx,dCurrControlState);
    return;
}

bool CRoomData::trialFinished()
{
    return timeIdx()>=_vpPhiGamma[0]->GetControlValues().Len();
    if(timeIdx()>1)
    {
        CMatrix<double> mEye = eyePath().GetRow(timeIdx()-1).Transpose();
        CMatrix<double> mDiff =  mEye - targetPostPosition();
        double dDistance = mDiff.GetVecNorm();
        return dDistance<0.25;
    }
    else 
        return false;
}


void CRoomData::precompute()
{
    CData::precompute();
    _vpPhiGamma[0]->precompute();
    _vpPhiGamma[1]->precompute();

    setTrialDurationSecond() = _vpPhiGamma[0]->disturbance(0)->preRunTimeinSecs()+_vpPhiGamma[0]->disturbance(0)->trialLengthinSecs()+_vpPhiGamma[0]->disturbance(0)->extraSecs();
}

void CRoomData::precomputeEachTrial()
{
    // precompute heading offset
    // Radius from (-10,-5)||(5,10)
    setHeadingOffset() = (RandLim(-1.,1.)>0? -1:1)*RandLim(headingOffsetBoundsDeg()[0]*DEG2RAD,headingOffsetBoundsDeg()[1]*DEG2RAD);
    setGazeOffset()    = (RandLim(-1.,1.)>0? -1:1)*RandLim(gazeOffsetBoundsDeg()[0]*DEG2RAD,gazeOffsetBoundsDeg()[1]*DEG2RAD);
    setGazePerturbationAxisOffsetRad() = (RandLim(-1.,1.)>0? -1:1)*abs(gazePerturbationAxisOffsetRad());

    _vpPhiGamma[0]->precomputeEachTrial();
    _vpPhiGamma[1]->precomputeEachTrial();
    precomputePathDirect();
}

void CRoomData::precomputePathDirect()
{
    setTimeIdx() = 0;
    int nTotalFrame = _vpPhiGamma[0]->GetControlValues().Len();
    setEyePath().SetSize(nTotalFrame,3,0);
    setObserverDirect().SetSize(nTotalFrame,3,0);
    SetRoll()            .SetSize(nTotalFrame,1,0);
    SetYaw()             .SetSize(nTotalFrame,1,0);
    SetPitch()           .SetSize(nTotalFrame,1,0);
    SetObserverUpVecRef().SetSize(nTotalFrame,3,0);
    SetObserverUpVec()   .SetSize(nTotalFrame,3,0);
    //int nTrialIdx = trialIdx();
    //double dForwardVelocity = GetRunScript()(nTrialIdx,1);
}

void CRoomData::clear()
{
    CData::clear();

    for(int i=0;i<(int)_vpPhiGamma.size();i++)
    {
        if(_vpPhiGamma[i])
            delete _vpPhiGamma[i];
    }
    _vpPhiGamma.clear();

}

void CRoomData::computeEyePositionAndGazing(CMatrix<double>& mEye_,CMatrix<double>& mGazingPoint_,int nCurrentFrame_)
{
    ////disturb lateral position
    //int nControlOrder = GetRunScript()(trialIdx(),0);
    ////compute eye postion
    //if(nCurrentFrame_==0)
    //{
    //    mEye_ = eyePath().GetRow(0);
    //    double dEx = phiGamma(nControlOrder)->GetOutputValues()(nCurrentFrame_,0);
    //    mEye_(0) = dEx;
    //    setEyePath().SetRow(nCurrentFrame_,mEye_);
    //}
    //else if(nCurrentFrame_>=1)
    //{ 
    //    ////perturbate lateral position
    //    CMatrix<double> mHeading(1,3,0);
    //    mEye_ = eyePath().GetRow(nCurrentFrame_-1);
    //    double dVeclocityPerframe = GetRunScript()(trialIdx(),1)/phiGamma(nControlOrder)->frameRate();
    //    double dEx1 = phiGamma(nControlOrder)->GetOutputValues()(nCurrentFrame_,0);
    //    double dEx0 = phiGamma(nControlOrder)->GetOutputValues()(nCurrentFrame_-1,0);
    //    double dVelX = dEx1 - dEx0; 
    //    double dSinVal = dVelX/dVeclocityPerframe;
    //    if(abs(dSinVal)>1.0)
    //        throw exception("Lateral speed is larger than forward speed");

    //    double dEz = sqrt(dVeclocityPerframe*dVeclocityPerframe-dVelX*dVelX);
    //    mEye_(0) = dEx1;
    //    mEye_(2) += dEz;
    //    setEyePath().SetRow(nCurrentFrame_,mEye_);
    //}

    /* disturb heading */
    int nControlOrder = GetRunScript()(trialIdx(),0);
    double dAngleRad;
    //compute eye postion
    if(nCurrentFrame_==0)
    {
        mEye_ = eyePath().GetRow(0);
        double dEx = phiGamma(nControlOrder)->GetOutputValues()(nCurrentFrame_,0);
        dAngleRad = dEx; //atan((dEx));//+virtualHeadingErrorDegree()*DEG2RAD);
        double dBound = headingBoundDegree()*DEG2RAD;
        dAngleRad = dAngleRad > dBound? dBound:dAngleRad;
        dAngleRad = dAngleRad < -dBound? -dBound:dAngleRad;
    }
    else if(nCurrentFrame_>=1)
    {
        CMatrix<double> mHeading(1,3,0);
        mEye_ = eyePath().GetRow(nCurrentFrame_-1);
        double dVeclocityPerframe = GetRunScript()(trialIdx(),1)/phiGamma(nControlOrder)->frameRate();
        double dEx = phiGamma(nControlOrder)->GetOutputValues()(nCurrentFrame_,0);
        if(abs(phiGamma(nControlOrder)->disturbance(0)->disturbanceGain())<0.000000001)
            dAngleRad = dEx + headingOffset();
        else
            dAngleRad = dEx; //atan((dEx));//+virtualHeadingErrorDegree()*DEG2RAD);
        double dBound = headingBoundDegree()*DEG2RAD;
        dAngleRad = dAngleRad > dBound? dBound:dAngleRad;
        dAngleRad = dAngleRad < -dBound? -dBound:dAngleRad;
        mHeading(0) = sin(dAngleRad);
        mHeading(2) = cos(dAngleRad);
        mEye_ += mHeading*dVeclocityPerframe;
        setEyePath().SetRow(nCurrentFrame_,mEye_);
    } 

    /*disturb gaze w.r.t. heading*/
    /*CMatrix<double> mGazing;

    //if(nCurrentFrame_==0)
    //{
    //    mGazing.SetSize(1,3,0);
    //    mGazing(2) = 1;
    //}
    //else if(nCurrentFrame_>=1)
    {
        mGazing.SetSize(1,3,0);

        double dDx = phiGamma(nControlOrder)->GetOutputValues()(nCurrentFrame_,1);
        if (abs(phiGamma(nControlOrder)->disturbance(1)->disturbanceGain())<0.000000001)
        {
            dDx += gazeOffset(); 
            //disturbed w.r.t. heading
            mGazing(0) = sin( dDx+dAngleRad);
            mGazing(2) = cos( dDx+dAngleRad);
        } 
        else
        {
            mGazing(0) = sin( dDx+dAngleRad+gazePerturbationAxisOffsetRad() );
            mGazing(2) = cos( dDx+dAngleRad+gazePerturbationAxisOffsetRad() );
        }
        
        //double dTargetAngle = atan(mEye_(0)/(gfxFar()-3.));
        //disturbed w.r.t. direction toward the target.
        //mGazing(0) = sin( dAngleRad-dTargetAngle);
        //mGazing(2) = cos( dAngleRad-dTargetAngle);

        setObserverDirect().SetRow(nCurrentFrame_,mGazing);
    }
    SetYaw()(nCurrentFrame_) =  -atan2(mGazing(0),mGazing(2))+M_PI;
    SetRoll()(nCurrentFrame_) =  0;
    SetPitch()(nCurrentFrame_) =  0;

    mGazingPoint_ = mEye_ + mGazing;
    return;*/

    //gaze w.r.t. Z
    CMatrix<double> mGazing;

    //if(nCurrentFrame_==0)
    //    mGazing.SetSize(1,3,0);
    //    mGazing(2) = 1;
    //}
    //else if(nCurrentFrame_>=1)
    {
        mGazing.SetSize(1,3,0);

        double dDx = phiGamma(nControlOrder)->GetOutputValues()(nCurrentFrame_,1);
        if (abs(phiGamma(nControlOrder)->disturbance(1)->disturbanceGain())<0.000000001)
        {
            dDx += gazeOffset(); 
        } 
        //disturbed w.r.t. heading
        mGazing(0) = sin( dDx);
        mGazing(2) = cos( dDx);
        //double dTargetAngle = atan(mEye_(0)/(gfxFar()-3.));
        //disturbed w.r.t. direction toward the target.
        //mGazing(0) = sin( dAngleRad-dTargetAngle);
        //mGazing(2) = cos( dAngleRad-dTargetAngle); 

        setObserverDirect().SetRow(nCurrentFrame_,mGazing);
    }
    SetYaw()(nCurrentFrame_) =  -atan2(mGazing(0),mGazing(2))+M_PI;
    SetRoll()(nCurrentFrame_) =  0;
    SetPitch()(nCurrentFrame_) =  0;

    mGazingPoint_ = mEye_ + mGazing;
    return;
}

void CRoomData::saveToFile()
{
    CFileIO cFIO;
    // check existence data directory and create if necessary
    cFIO.CreateCWDir(string("\\data")); // do not add trailing slash! (prefixing a slash is not necessary but ok

    QString qstrFullOuptputFileName;
    fullOutputFileName(trialIdx(),&qstrFullOuptputFileName);

    // create file
    cFIO.OpenFileWrite(qstrFullOuptputFileName.toStdString(),true);
    // inputs from file and other fixed parameters
    cFIO.SetFieldWidth() = 10;

    /*cFIO.WriteStrValToFile(string("Probe-Idx"));
    cFIO.WriteTab();
    cFIO.WriteStrValToFile(string("Control"));
    cFIO.WriteTab();
    cFIO.WriteStrValToFile(string("Dist heading"));
    cFIO.WriteTab();
    cFIO.WriteStrValToFile(string("Dist gaze"));
    cFIO.WriteTab();
    cFIO.WriteStrValToFile(string("Heading"));
    cFIO.WriteTab();
    cFIO.WriteStrValToFile(string("Gazing"));
    cFIO.WriteTab();
    cFIO.WriteStrValToFile(string("Path X"));
    cFIO.WriteTab();
    cFIO.WriteStrValToFile(string("Path Z"));
    cFIO.WriteEndl();*/
    int nControlOrder = GetRunScript()(trialIdx(),0);
    int nStartFrame = phiGamma(nControlOrder)->disturbance(0)->preRunFrames();
    
    for(int i=nStartFrame;i< phiGamma(nControlOrder)->GetControlValues().Rows();i++)
    {
        cFIO.WriteIntValToFile(i);
        cFIO.WriteTab();
        cFIO.WriteDblValToFile(phiGamma(nControlOrder)->GetControlValues()(i));
        cFIO.WriteTab();
        cFIO.WriteDblValToFile(phiGamma(nControlOrder)->disturbance(0)->disturbanceValues()(i)); //x
        cFIO.WriteTab();
        cFIO.WriteDblValToFile(phiGamma(nControlOrder)->disturbance(1)->disturbanceValues()(i)); //q
        cFIO.WriteTab();
        cFIO.WriteDblValToFile(phiGamma(nControlOrder)->GetOutputValues()(i,0));    // x
        cFIO.WriteTab();
        cFIO.WriteDblValToFile(phiGamma(nControlOrder)->GetOutputValues()(i,1)+gazePerturbationAxisOffsetRad());    // theta
        cFIO.WriteTab();
        cFIO.WriteDblValToFile(eyePath()(i,0));
        cFIO.WriteTab();
        cFIO.WriteDblValToFile(eyePath()(i,2));
        cFIO.WriteEndl();
    }
    cFIO.CloseFile();
}

void CRoomData::afterCompute()
{
    return;
}

bool CRoomData::preRunFinished()
{
    int nControlOrder = GetRunScript()(trialIdx(),0);
    if (timeIdx() <= phiGamma(nControlOrder)->disturbance(0)->preRunFrames()) 
        return false;
    else
        return true;
}

void CRoomData::parser( const string& strCondition_, vector<int>* pvConds_ )
{
    CPathData::parser(strCondition_,pvConds_);
}

void CRoomData::CheckOutputFile()
{
    CFileIO cFIO;
    // check existence data directory and create if necessary
    cFIO.CreateCWDir(string("\\data")); // do not add trailing slash! (prefixing a slash is not necessary but ok

    //char cFileName[80];
    QString qstrFileName;
    for(int nTrialIdx = 0;nTrialIdx<(int)conditionScript().size();nTrialIdx++)
    {
        fullOutputFileName(nTrialIdx,&qstrFileName);
        cFIO.CheckFileExist(qstrFileName.toStdString());
    }

    return;
}

void CRoomData::fullOutputFileName(int nTrial_,QString* pFullOutputFileName_) const
{
    pFullOutputFileName_->clear();
    string strCond = conditionScript()[nTrial_];
    double dForwardVelocity = GetRunScript()(nTrial_,1);
    double dTrialIdx = GetRunScript()(nTrial_,2);
    int nNumDots = GetRunScript()(nTrial_,3);

    // create filename: ClassName_subjectName_trialIdx_v3_d1_UG+CL+T.txt

    string strHead = "2dist";// due to the requirement of dee's matlab file.
    int nCondition = conditionNumScript()[nTrial_];
    if(strCond == "SG+T")//empty ground 0
        if(nCondition != 0)
            throw exception("The condition doesn't match predefined convention.");
    else if(strCond == "TG+T")// texture ground 1
        if(nCondition != 1)
            throw exception("The condition doesn't match predefined convention.");
    else if(strCond == "TG+P+T"||strCond == "P+TG+T") // texture post 2
        if(nCondition != 2)
            throw exception("The condition doesn't match predefined convention.");
    else if((strCond == "UGZ+T"||strCond == "T+UGZ") && nNumDots == 200) //training 3
        if(nCondition != 3)
            throw exception("The condition doesn't match predefined convention.");
    else if((strCond == "UGZ+T"||strCond == "T+UGZ") && nNumDots == 80 ) //UGZ 80 4
        if(nCondition != 4)
            throw exception("The condition doesn't match predefined convention.");
    else if((strCond == "UG+T"||strCond == "T+UG") && nNumDots == 100 ) //UG 100 5
        if(nCondition != 5)
            throw exception("The condition doesn't match predefined convention.");
    else if((strCond == "UGZ+T"||strCond == "T+UGZ") && nNumDots == 100 ) //UG 100 6
        if(nCondition != 6)
            throw exception("The condition doesn't match predefined convention.");
    else if((strCond == "UGZ+T"||strCond == "T+UGZ") && nNumDots == 300 ) //UGZ 300 7
        if(nCondition != 7)
            throw exception("The condition doesn't match predefined convention.");

    *pFullOutputFileName_= QString("%1_%2_%3_%4.txt")
        .arg(strHead.c_str())
        .arg(outputFileName().c_str())
        .arg(nCondition)
        .arg(dTrialIdx);
    return;
}

